

#include<stdio.h>
#include<Arcroll.h>
#include<Mesh_IO/Mesh_IO.h>
#include<Mesh/libcell_iterator.h>
#include<Math/LB_Quaternions.h>

#include "ppf.h"
#include "nfd.h"


#define _ReadPly_ LibCell_ReadPly_

#define _ReadPcd_ LibCell_ReadPcd_
#define quote lib_cell_quote
/********
 * 鼠标左键控制视角旋转
 * 鼠标右键控制视角平移
 * 鼠标滑轮控制视角前进，后退
 * // 当鼠标选中点云时， Ctrl键加鼠标滑轮单独对此点云放大缩小 
 * 当鼠标选中点云时，d键，隐藏此点云。 
 * b键，恢复被隐藏的点云
 * ctrl+d 隐藏所有按钮
 * ctrl + b显示所有按钮
 * 
 * 
 * 
 * 
 * 
 * 
 * 
 * 
 * 
 * 
 * 
 * 
 * 
***/


static  char * get_file_suffix(char *filename,char delimit)
{
    int len=strlen(filename);
    for(int i=1;i<len;i++)
    {
        if(filename[len-i]==delimit)
        {
            return (filename+len-i+1);
        }

    } 
    return NULL;
}

static inline int my_read_mesh_file(Mesh*m,char* filename)
{
    char* suffix=get_file_suffix(filename,'.');
    if(suffix==NULL)
    {
        return 0;
    }
    if(strcmp(suffix,"ply")==0)
    {
        _ReadPly_(m,filename);
    }
    else if(strcmp(suffix,"pcd")==0)
    {
        _ReadPcd_(m,filename);
    }

    return 1;
}



char* test_select_file()
{
    nfdchar_t *outPath = NULL;
    //nfdresult_t result=NFD_PickFolder( NULL,&outPath);
	//nfdresult_t result =NFD_SaveDialog( NULL, NULL, &outPath );
    nfdresult_t result = NFD_OpenDialog( NULL, NULL, &outPath ); 
    if ( result == NFD_OKAY ) {
        puts("Success!");
        printf("%s\n",outPath);

        return outPath;
    }
    else if ( result == NFD_CANCEL ) {
        //puts("User pressed cancel.");
    }
    else {
        printf("Error: %s\n", NFD_GetError() );
    }
    return outPath;
}
char* test_save()
{
    nfdchar_t *outPath = NULL;
    //nfdresult_t result=NFD_PickFolder( NULL,&outPath);
	nfdresult_t result =NFD_SaveDialog( NULL, NULL, &outPath );
    //nfdresult_t result = NFD_OpenDialog( NULL, NULL, &outPath ); 
    if ( result == NFD_OKAY ) {
        puts("Success!");
        printf("%s\n",outPath);

        return outPath;
    }
    else if ( result == NFD_CANCEL ) {
        //puts("User pressed cancel.");
    }
    else {
        printf("Error: %s\n", NFD_GetError() );
    }
    return outPath;
}

typedef struct Pose_Annotation_Animation{
//默认播放4秒

    int is_play; 
    float begin_time;
    // temp_m 储存场景的矩阵
    //temp_mi 场景矩阵的逆
    //temp_m1要变换插值的矩阵
    //temp_m2 模型原来的矩阵
    LB_Matrix* temp_m,*temp_mi,*temp_m1,*temp_m2;
//统计变换次数
    int n;
    double  angle,axis[3];

}Pose_Annotation_Animation;

static inline void pose_annotation_animation_init(Pose_Annotation_Animation* paa )
{
    paa->is_play=0;
    paa->begin_time=0;
    paa->temp_m=NULL;paa->temp_mi=NULL;paa->temp_m1=NULL;paa->temp_m2=NULL;
    paa->n=0;
    paa->angle=0;
    memset(paa->axis,0,sizeof(double )*3);

}

typedef struct Select_Points{
    Viewer_Something * vs;
    int mark;
} Select_Points;
static inline void select_points_init(Select_Points* sp)
{

    sp->vs=NULL;
    sp->mark=0;
}


typedef struct Pose_Annotation_Tool_Prop{

    float old_mouse_coord[2];

	char* model_name;
	Mesh * model;

	char* scene_name;
	Mesh* scene;
	Viewer_Something* v_model;
	Viewer_Something* v_scene;

	Viewer_Something* input_str;
    int is_input;	

	Viewer_Something *record_str;
	Viewer_Something *suggest_str;
	int is_move_piece;
    Pose_Annotation_Animation paa;
    
    Ppf_Recommand_Params prp;	
	Node* n_vums;
    Node* n_poses;
    // temp_m 是查看记录位姿时，立即保存当前位姿，作为还原。
    //
    // synchronized_mat是编辑记录位姿用的
    //****
    // ani_temp 是用来播放动画时，立即储存当前位姿。
//***************
    LB_Matrix* synchronized_mat,* temp_m;
    //***
// 选择的对应6个点
    //****

    Select_Points select_ps[6];

    Viewer_Something * v_sp;

	Viewer_Opengl_Interpreter* voi;
	Viewer_World* vw;


	void * prop;

}Pose_Annotation_Tool_Prop;
Viewer_Something*  viewer_add_button(Viewer_World* vw,char * image ,float*pos , 
    void (call_fun)(Viewer_Intera*),void* prop );


static inline void pose_annotation_tool_prop_init(Pose_Annotation_Tool_Prop* patp)
{
	memset(patp->old_mouse_coord,0,sizeof(float)*2);

	patp->model_name=NULL;
	patp->model=NULL;
	patp->scene_name=NULL;
	patp->scene=NULL;
	patp->v_model=NULL;
	patp->v_scene=NULL;
    patp->input_str=NULL;
    patp->record_str=NULL;
    patp->suggest_str=NULL;

    patp->is_input=0;
    patp->temp_m=NULL;
    patp->n_poses=NULL;


    patp->synchronized_mat=NULL;
    patp->temp_m=NULL;
    pose_annotation_animation_init(&(patp->paa));

    ppf_recommand_params_init(&(patp->prp));

	patp->is_move_piece=0;
	patp->n_vums=NULL;

    for(int i=0;i<6;i++)
    {
       select_points_init(  &( patp->select_ps[i]));
    }
    patp->v_sp=NULL;
    //memset(patp->select_ps,0,sizeof(Select_Points))
	
    patp->voi=NULL;
	patp->vw=NULL;
	patp->prop=NULL;
}

static inline void pose_annotation_tool_prop_free(Pose_Annotation_Tool_Prop* patp)
{
	if(patp==NULL)
	{
		return;
	}
	SAFE_FREE(patp->model_name);
	if(patp->model!=NULL)
	{
		Mesh_free(patp->model);
		SAFE_FREE(patp->model);
	}
	
	
	SAFE_FREE(patp->scene_name);
	if(patp->scene!=NULL)
	{
		Mesh_free(patp->scene);
		SAFE_FREE(patp->scene);
	}
	
	patp->v_model=NULL;
	patp->v_scene=NULL;
    free_node(patp->n_poses);
    patp->n_poses=NULL;

	patp->voi=NULL;

	free(patp);

}
void default_button_call_fun (Viewer_Intera* vi)
{
    Viewer_UI_Mesh* vum=(Viewer_UI_Mesh*)(vi->representation);
    Viewer_Something * vs=vum->texture;
    Interactor_GlobalInfo* g_info=vi->g_info;  

    Interactor_PickInfo ipi=g_info->ipi;

    if(g_info->mouse_button==VIEWER_MOUSE_BUTTON_LEFT&&g_info->mouse_action==VIEWER_PRESS)
    {
        if(ipi.vs!=NULL&&vum==ipi.vs->evolution)
        {
            vum->change_color=0.5;
           	
           	//char * str[100]={0};
           	printf("请输入:\n");
           	//scanf("%s",str);
           	//printf("%s\n",str);		 
        }
    } 
    else if(g_info->mouse_button==VIEWER_RELEASE)
    {
            vum->change_color=1.0;   
    }
}


void ouput_record_poses(char* filename,Pose_Annotation_Tool_Prop* patp)
{
    if(filename==NULL|| patp->model_name==NULL||patp->scene_name==NULL||patp->n_poses==NULL)
    {
        return;
    }
#ifdef _WIN32
    FILE *outfile;
    fopen_s(&outfile,filename,"w");
#else
    FILE *outfile=fopen(filename,"w");
#endif 
    if(!outfile)
    {
        printf("cant open this file\n");
        return ;
    }
    fseek(outfile,0,SEEK_SET);


    for(Node* nit= patp->n_poses;nit!=NULL;nit=(Node*)(nit->Next))
    {
        Viewer_Something* vs1=(Viewer_Something*)(nit->traits);
        if(vs1->disappear==1)
        {
            continue;
        }
        LB_Matrix* mat=(LB_Matrix*)(nit->value);
        float* data=(float*)(mat->data);
        fprintf(outfile,"%s %s ",patp->model_name,patp->scene_name);
        for(int i=0;i<4;i++)
        {
            for(int j=0;j<4;j++)
            {
                fprintf(outfile,"%.8lf ",data[i*4+j] );

            }
        }
        fprintf(outfile,"\n");
    } 
    fclose(outfile);
}

void clear_record_poses(Pose_Annotation_Tool_Prop*patp)
{
    if(patp->n_poses==NULL)
    {
        return;
    }
    if(patp->temp_m!=NULL)
    {
        lb_matrix_free(patp->temp_m);
        patp->temp_m=NULL;
    }
    if(patp->synchronized_mat!=NULL)
    {
        lb_matrix_free(patp->synchronized_mat);
        patp->synchronized_mat=NULL; 
    }
    for(Node* nit=patp->n_poses;nit!=NULL;nit=(Node*)(nit->Next))
    {
        LB_Matrix* mat=(LB_Matrix*)(nit->value);
        Viewer_Something* vs=(Viewer_Something*)(nit->traits);
        patp->vw->remove_something(patp->vw,vs); 
        lb_matrix_free(mat);
    } 
    free_node(patp->n_poses);
    patp->n_poses=NULL;

}

void output_pose_call_fun (Viewer_Intera* vi)
{
    Viewer_UI_Mesh* vum=(Viewer_UI_Mesh*)(vi->representation);
    Viewer_Something * vs=vum->texture;
    Interactor_GlobalInfo* g_info=vi->g_info;  

    Interactor_PickInfo ipi=g_info->ipi;
    Pose_Annotation_Tool_Prop*patp=(Pose_Annotation_Tool_Prop*)(vi->prop);

    if(g_info->mouse_button==VIEWER_MOUSE_BUTTON_LEFT&&g_info->mouse_action==VIEWER_PRESS)
    {
        if(ipi.vs!=NULL&&vum==ipi.vs->evolution)
        {
            vum->change_color=0.5;

            if(patp->n_poses!=NULL)
            {

                char * file=test_save();
                ouput_record_poses(file,patp); 
            }
        }
    } 
    else if(g_info->mouse_button==VIEWER_RELEASE)
    {
            vum->change_color=1.0;   
    }
}




void select_workpiece_call_fun (Viewer_Intera* vi)
{
    Viewer_UI_Mesh* vum=(Viewer_UI_Mesh*)(vi->representation);
    Viewer_Something * vs=vum->texture;
    Interactor_GlobalInfo* g_info=vi->g_info;  

    Interactor_PickInfo ipi=g_info->ipi;
    Pose_Annotation_Tool_Prop*patp=(Pose_Annotation_Tool_Prop*)(vi->prop);
    if(g_info->mouse_button==VIEWER_MOUSE_BUTTON_LEFT&&g_info->mouse_action==VIEWER_PRESS)
    {
        if(ipi.vs!=NULL&&vum==ipi.vs->evolution)
        {
            vum->change_color=0.5;

            printf("slect model\n");

            char * file=test_select_file();
           	if(file!=NULL)
           	{
           		int mark=0;
           		if(patp->model_name==NULL)
           		{
           			patp->model_name=file;
           			mark=1;
           		}
           		else if(strcmp(patp->model_name,file)!=0)
           		{
           			SAFE_FREE(patp->model_name);
           			patp->model_name=file;
           			if(patp->v_model!=NULL&&patp->vw!=NULL)
           			{
           				Mesh_free(patp->model);
           				SAFE_FREE(patp->model);
           				//patp->vw->print_self(patp->vw);
           				patp->vw->remove_something(patp->vw,patp->v_model);
           				//patp->vw->print_self(patp->vw);

           				patp->v_model=NULL;
           			}	
           			mark=1;
           		}

           		if(mark==1)
           		{

                    clear_record_poses(patp); 
           			patp->model=(Mesh*)malloc(sizeof(Mesh));
           			Mesh_init(patp->model);
           			if(my_read_mesh_file(patp->model,patp->model_name))
                    {
                        float*center= (float*)malloc(sizeof(float)*3);
                        patp->model->prop=center;
                        memset(patp->model->prop,0,sizeof(float)*3);



                        Node*n=patp->vw->create_something(patp->vw,"Points");

                        auto vs=(Viewer_Something*)(n->value);
                        auto vp=(Viewer_Points*)(vs->evolution);

                    //printf("num v: %d\n",mesh.num_v(&mesh));
                        vp->Data_rows=patp->model->num_v(patp->model);
                        printf("%d \n",vp->Data_rows);

                        vp->Data=(float*)malloc(sizeof(float)*3*(vp->Data_rows));
                        vp->set_color(vp,0.9,0.3,0.7,1.0);
                        int i=0;


                        for(auto vit=patp->model->v_begin(patp->model);vit!=patp->model->v_end(patp->model);vit++)
                        {
                            if(i>=vp->Data_rows)
                            {
                                printf("i %d\n",i);
                                break;
                            }
                            for(int j=0;j<3;j++)
                            {
                                center[j]+=quote(vit)->point[j];

                                vp->Data[i*3+j]=quote(vit)->point[j];
                            }
                            i++;
                        }

                        center[0]/=(float)i;center[1]/=(float)i;center[2]/=(float)i;
                        printf("center :%lf %lf %lf\n",center[0],center[1],center[2]);

                    //vp->Data[i*3+0]=center[0];vp->Data[i*3+1]=center[1];vp->Data[i*3+2]=center[2];
                    //vp->color[i*4+0]=0.0;vp->color[i*4+3]=1.0;


                        vp->pointsize=2.5;
                        free_node(n);   
                        patp->v_model=vs;

                        if(patp->v_scene!=NULL)
                        {
                            vp=(Viewer_Points*)(patp->v_scene->evolution);
                            vp->mat->identity(vp->mat);
                        }   
                    }
           		}	
           		patp->voi->update_data(patp->voi);
           	} 
            
        }
    } 
    else if(g_info->mouse_button==VIEWER_RELEASE)
    {
            vum->change_color=1.0;   
    }
}

void select_scene_call_fun (Viewer_Intera* vi)
{
    Viewer_UI_Mesh* vum=(Viewer_UI_Mesh*)(vi->representation);
    Viewer_Something * vs=vum->texture;
    Interactor_GlobalInfo* g_info=vi->g_info;  

    Interactor_PickInfo ipi=g_info->ipi;
    Pose_Annotation_Tool_Prop*patp=(Pose_Annotation_Tool_Prop*)(vi->prop);


    if(g_info->mouse_button==VIEWER_MOUSE_BUTTON_LEFT&&g_info->mouse_action==VIEWER_PRESS)
    {
        if(ipi.vs!=NULL&&vum==ipi.vs->evolution)
        {

            printf("select_scene\n");
            vum->change_color=0.5;

            char *file=test_select_file();
            if(file!=NULL)
           	{
           		int mark=0;
           		if(patp->scene_name==NULL)
           		{
           			patp->scene_name=file;
           			mark=1;
           		}
           		else if(strcmp(patp->scene_name,file)!=0)
           		{
           			SAFE_FREE(patp->scene_name);
           			patp->scene_name=file;
           			if(patp->v_scene!=NULL&&patp->vw!=NULL)
           			{
           				Mesh_free(patp->scene);
           				SAFE_FREE(patp->scene);
           				//patp->vw->print_self(patp->vw);
           				patp->vw->remove_something(patp->vw,patp->v_scene);
           				//patp->vw->print_self(patp->vw);
           				patp->v_scene=NULL;
           			}	
           			mark=1;
           		}

           		if(mark==1)
           		{

                    clear_record_poses(patp); 
 
           			patp->scene=(Mesh*)malloc(sizeof(Mesh));
           			Mesh_init(patp->scene);
           			 
                    if(my_read_mesh_file(patp->scene,patp->scene_name))
           			{
                        Node*n=patp->vw->create_something(patp->vw,"Points");

                        auto vs=(Viewer_Something*)(n->value);
                        auto vp=(Viewer_Points*)(vs->evolution);

                    //printf("num v: %d\n",mesh.num_v(&mesh));
                        vp->Data_rows=patp->scene->num_v(patp->scene);
                        printf("%d \n",vp->Data_rows);

                        vp->Data=(float*)malloc(sizeof(float)*3*vp->Data_rows);
                        vp->set_color(vp,0.3,0.9,0.4,1.0);
                        int i=0;


                        for(auto vit=patp->scene->v_begin(patp->scene);vit!=patp->scene->v_end(patp->scene);vit++)
                        {
                            if(i>=vp->Data_rows)
                            {
                                printf("i %d\n",i);
                                break;
                            }
                            for(int j=0;j<3;j++)
                            {
                                vp->Data[i*3+j]=quote(vit)->point[j];
                            }
                            i++;
                        }

                        vp->pointsize=2.5;
                        free_node(n);   
                        patp->v_scene=vs;
                        if(patp->v_scene==NULL)
                        {

                            printf("select_scene vs is NULL\n");
                        }
                        if(patp->v_model!=NULL)
                        {
                            vp=(Viewer_Points*)(patp->v_model->evolution);

                        // for(int i=0;i<4;i++)
                        // {
                        //     for(int j=0;j<4;j++)
                        //     {
                        //         ((float*)(vp->mat->data))[i*4+j]=0;
                        //     }
                        //     ((float*)(vp->mat->data))[i*4+i]=1.0;

                        // } 

                            vp->mat->identity(vp->mat);
                        }   

                    }
           			
           		}	
           		patp->voi->update_data(patp->voi);

           	} 
            
        }
    } 
    else if(g_info->mouse_button==VIEWER_RELEASE)
    {
            vum->change_color=1.0;   
    }
  
}


float* record_position(int n)
{
    int row=n/20,col=n%20;

    float* re=(float*)malloc(sizeof(float)*12);
     


    float center[3]={0.9-row*0.07,0.9-col*0.07,1.0 }; 


    float pos[12]={center[0]-0.03,center[1]-0.03,1.0,
        center[0]+0.03,center[1]-0.03,1.0,
        center[0]+0.03,center[1]+0.03  ,1.0,
        center[0]-0.03,center[1]+0.03,1.0
    };

    memmove(re,pos,sizeof(float)*12);

    return re;
}


void record_pose_small_parts(Viewer_Intera*vi)
{
    Viewer_UI_Mesh* vum=(Viewer_UI_Mesh*)(vi->representation);
    Viewer_Something * vs=vum->texture;
    Interactor_GlobalInfo* g_info=vi->g_info;  

    Interactor_PickInfo ipi=g_info->ipi;
    Pose_Annotation_Tool_Prop*patp=(Pose_Annotation_Tool_Prop*)(vi->prop);

    if(g_info->mouse_button==VIEWER_MOUSE_BUTTON_LEFT&&g_info->mouse_action==VIEWER_PRESS)
    {
        if(ipi.vs!=NULL&&vum==ipi.vs->evolution)
        {

            if(vum->change_color<0.9)
            {
                //patp->is_move_piece=0;
                if(patp->temp_m!=NULL)
                {
                    LB_Matrix* temp_m=(LB_Matrix*)malloc(sizeof(LB_Matrix));
                    lb_matrix_init_float(temp_m,4,4);
                    Viewer_Points* vp=(Viewer_Points*)(patp->v_scene->evolution);

                    temp_m->copy_data(temp_m,vp->mat);
                    temp_m->mult(temp_m,patp->temp_m);
                
                    vp=(Viewer_Points*)(patp->v_model->evolution);
                    vp->mat->copy_data(vp->mat,temp_m);  

                    lb_matrix_free(temp_m);
                    lb_matrix_free(patp->temp_m);
                }

                patp->synchronized_mat=NULL; 

                patp->temp_m=NULL;

                vum->change_color=1.0;
            }
            else
            {
                if(patp->synchronized_mat!=NULL)
                {
                    for(Node* nit=patp->n_poses;nit!=NULL;nit=(Node*)(nit->Next))
                    {
                        if(nit->value==patp->synchronized_mat)
                        {
                            Viewer_Something* vs1= (Viewer_Something*)(nit->traits);
                            ((Viewer_UI_Mesh*)(vs1->evolution))->change_color=1.0;
                            patp->synchronized_mat=NULL;
                            break; 
                        }

                    }
                } 
                else {

                    if(patp->temp_m!=NULL)
                    {
                        lb_matrix_free(patp->temp_m);
                    }  
                    if(patp->v_scene!=NULL&&patp->v_model!=NULL)
                    {
                        Viewer_Points*vp=(Viewer_Points*)(patp->v_scene->evolution);
                        patp->temp_m=vp->mat->inverse(vp->mat);
                        vp=(Viewer_Points*)(patp->v_model->evolution);
                        patp->temp_m->mult(patp->temp_m, vp->mat);
                    }
                }
                

                vum->change_color=0.5;
                Node* nit=(Node*)(ipi.vs->prop);
                patp->synchronized_mat=(LB_Matrix*)(nit->value);

                LB_Matrix* temp_m=(LB_Matrix*)malloc(sizeof(LB_Matrix));
                lb_matrix_init_float(temp_m,4,4);
                Viewer_Points* vp=(Viewer_Points*)(patp->v_scene->evolution);

                temp_m->copy_data(temp_m,vp->mat);
                temp_m->mult(temp_m,patp-> synchronized_mat);
                
                vp=(Viewer_Points*)(patp->v_model->evolution);
                vp->mat->copy_data(vp->mat,temp_m);  

                lb_matrix_free(temp_m);
            }
        }
    }
}



static inline double* get_input_matrix_from_str(wchar_t*wstr  )
{

    int len=wcslen(wstr);
    char  str[990]={0};
    wcstombs(str, wstr,len );

    //printf("%s\n",str);
    int n=0;
    char** s_str= split(str,' ',&n);
    //printf("%d\n",n);

    double* re=NULL;


    if(n==16)
    {
        re=(double*)malloc(sizeof(double)*16);
        for(int i=0;i<16;i++)
        {
            sscanf(s_str[i],"%lf",&(re[i]) );
            printf("%lf\n",re[i]);

        }  

    }
    for(int i=0;i<n;i++)
    {
        free(s_str[i]);
    }
    free(s_str);

    return re;
}





void input_matrix_callback(Viewer_Intera* vi)
{
    Viewer_UI_Mesh* vum=(Viewer_UI_Mesh*)(vi->representation);
    Viewer_Something * vs=vum->texture;
    Interactor_GlobalInfo* g_info=vi->g_info;  

    Interactor_PickInfo ipi=g_info->ipi;
    Pose_Annotation_Tool_Prop*patp=(Pose_Annotation_Tool_Prop*)(vi->prop);
    if(g_info->mouse_button==VIEWER_MOUSE_BUTTON_LEFT&&g_info->mouse_action==VIEWER_PRESS)
    {
        if(ipi.vs!=NULL&&vum==ipi.vs->evolution)
        {
            Viewer_Texts* vtexts= (Viewer_Texts*)(patp->input_str->evolution);
              
            if(vum->change_color<0.9)
            {

                if(patp->v_model!=NULL&&patp->v_scene!=NULL)
                {

                    double* mat_data=get_input_matrix_from_str(vtexts->str);

                    if(mat_data!=NULL)
                    {
                        //printf("not NULL\n");
                        LB_Matrix* temp_m=(LB_Matrix*)malloc(sizeof(LB_Matrix));
                        lb_matrix_init_float(temp_m,4,4);

                        float * data=(float*)(temp_m->data);

                        //printf("here \n");

                        // data[0]=mat_data[0];data[1]=mat_data[1];data[0]=mat_data[0];data[1]=mat_data[1];
                        // data[0]=mat_data[0];data[1]=mat_data[1]; data[0]=mat_data[0];data[1]=mat_data[1];

                        for(int k=0;k<16;k++)
                        {
                            data[k]=  mat_data[k];
                            //printf("%f %lf\n",data[k],mat_data[k] );
                        } 

                        LB_Matrix* temp_m1=(LB_Matrix*)malloc(sizeof(LB_Matrix));
                        lb_matrix_init_float(temp_m1,4,4);

                        Viewer_Points* vp1=(Viewer_Points*)(patp->v_scene->evolution);
                        temp_m1->copy_data(temp_m1,vp1->mat);

                        temp_m1->mult(temp_m1,temp_m);

                        vp1=(Viewer_Points*)(patp->v_model->evolution);
                        
                        vp1->mat->copy_data(vp1->mat,temp_m1); 



                        lb_matrix_free(temp_m);
                        lb_matrix_free(temp_m1);
                        free(mat_data);
                        printf("");

                    }
                   

                }



                //if(patp->is_input)  
                vtexts->xy[0] =-0.6;
                memset(vtexts->str,0,sizeof(wchar_t)*700);
                wcscpy(vtexts->str,L"                中原动力位姿标注");

                patp->is_input=0;
                vum->change_color=1.0;
            }            
            else if(vum->change_color>0.9)
            {
                if(patp->is_input==1)
                {
                    return;
                }
                memset(vtexts->str,0,sizeof(wchar_t)*700);

                patp->is_input=1;
                vum->change_color=0.5;
            }
        }
    }

}

static inline Ppf_Recommand_Params  get_ppf_recommand_param_from_str(wchar_t*wstr  )
{

    int len=wcslen(wstr);
    char  str[990]={0};
    wcstombs(str, wstr,len );

    int n=0;
    //char** s_str= split(str,' ',&n);

    
    Ppf_Recommand_Params  re;

    ppf_recommand_params_init(&re);

    // for(int i=0;i<4&&i< n;i++)
    // {
    //     sscanf();  

    // } 
    sscanf(str," %d %d %f %f", &(re.circle),&(re.is_inverse_normal), &(re.model_sample),&(re.scene_sample));

    printf("shuru : %d %d %lf %lf\n",re.circle,re.is_inverse_normal ,re.model_sample,re.scene_sample);

    return re;
}

void ppf_recommand_callback(Viewer_Intera* vi)
{
    Viewer_UI_Mesh* vum=(Viewer_UI_Mesh*)(vi->representation);
    Viewer_Something * vs=vum->texture;
    Interactor_GlobalInfo* g_info=vi->g_info;  

    Interactor_PickInfo ipi=g_info->ipi;
    Pose_Annotation_Tool_Prop*patp=(Pose_Annotation_Tool_Prop*)(vi->prop);


    if(g_info->mouse_button==VIEWER_MOUSE_BUTTON_LEFT&&g_info->mouse_action==VIEWER_PRESS)
    {
        if(ipi.vs!=NULL&&vum==ipi.vs->evolution)
        {
            Viewer_Texts* vtexts= (Viewer_Texts*)(patp->input_str->evolution);
              
            if(vum->change_color<0.9)
            {
                if(patp->v_model!=NULL&&patp->v_scene!=NULL)
                {
                    patp->prp=get_ppf_recommand_param_from_str(vtexts->str);
                    
                    int n[2]={patp->scene->num_v(patp->scene),patp->model->num_v(patp->model)    };
                    double**array_model=(double**)malloc(sizeof(double*)*n[1]), **array_scene=(double**)malloc(sizeof(double*)*n[0]);




                    int i=0;
                    for(auto vit=patp->scene->v_begin(patp->scene);vit!=patp->scene->v_end(patp->scene);vit++)
                    {
                        array_scene[i]=(double*)malloc(sizeof(double)*3);
                        memmove(array_scene[i],quote(vit)->point,sizeof(double)*3);

                        i++;
                    }
                    i=0;
                    for(auto vit=patp->model->v_begin(patp->model);vit!=patp->model->v_end(patp->model);vit++)
                    {
                        array_model[i]=(double*)malloc(sizeof(double)*3);
                        memmove(array_model[i],quote(vit)->point,sizeof(double)*3);
                        i++;
                    } 
                    void * hash_map;double ad_fac;

                    void* m2=compute_model_feature(array_model,n[1],&(patp->prp),&hash_map,&ad_fac);




                    Ppf_Out out=compute_scene_poses(array_scene,n[0],m2,&(patp->prp),hash_map,ad_fac);

                    delete hash_map;

                    SAFE_FREE(((Mesh*)(m2))->prop );
                    Mesh_free((Mesh*)(m2));
                    for(i=0;i<out.l;i++)
                    {
                        printf("once\n");
                        LB_Matrix* mat1=(LB_Matrix*)malloc(sizeof(LB_Matrix));
                        lb_matrix_init_float(mat1,4,4); 

                        for(int j=0;j<4;j++)
                        {
                            for(int k=0;k<4;k++)
                            {
                                ((float*)(mat1->data))[j*4+k]=out.mats[i][j*4+k];
                            }
                        }
                        

                        patp->n_poses=node_overlying(patp->n_poses,mat1);


                        int n_len=node_size(patp->n_poses);
                
                        float* pos= record_position(n_len-1);

                        Viewer_Something* vs1=viewer_add_button(patp->vw,NULL,pos,record_pose_small_parts,patp);
                        free(pos);

                        patp->n_poses->traits=vs1; 
                        vs1->prop=patp->n_poses;
                    }


                    ppf_out_free(&out);

                //     findmodel3D(array_scene,array_model,n,mat,scores,&(patp->prp));


                //     for(i=0;i<10;i++)
                //     {

                //         if(mat[i]==NULL)
                //         {
                //             break;
                //         }


                //         LB_Matrix* mat1=(LB_Matrix*)malloc(sizeof(LB_Matrix));
                //         lb_matrix_init_float(mat1,4,4); 

                //         for(int j=0;j<4;j++)
                //         {
                //             for(int k=0;k<4;k++)
                //             {

                //                 ((float*)(mat1->data))[j*4+k]=mat[i][k*4+j];
                //             }
                //         }
                        

                //         patp->n_poses=node_overlying(patp->n_poses,mat1);


                //         int n_len=node_size(patp->n_poses);
                
                //         float* pos= record_position(n_len-1);

                //         Viewer_Something* vs1=viewer_add_button(patp->vw,NULL,pos,record_pose_small_parts,patp);
                //         free(pos);

                //         patp->n_poses->traits=vs1;
                        
                //         vs1->prop=patp->n_poses;
                //         free(mat[i]);mat[i]=NULL;
                //     }

                    
                    for(i=0;i<n[0];i++)
                    {
                        free(array_scene[i]);
                    }
                    for(i=0;i<n[1];i++)
                    {
                        free(array_model[i]);
                    }
                    free(array_model);free(array_scene);


                    patp->voi->update_data(patp->voi);  
                }

                memset(vtexts->str,0,sizeof(wchar_t)*700);
                wcscpy(vtexts->str,L"                中原动力位姿标注");
                patp->is_input=0;
                vum->change_color=1.0;
            }            
            else if(vum->change_color>0.9)
            {
                if(patp->is_input==1)
                {
                    return;
                } 
                ppf_recommand_params_init (&(  patp->prp));
                memset(vtexts->str,0,sizeof(wchar_t)*700);

                patp->is_input=1;
                vum->change_color=0.5;
            }
        }
    }
}

void paly_ani_callback(Viewer_Intera*vi)
{

    Viewer_UI_Mesh* vum=(Viewer_UI_Mesh*)(vi->representation);
    Viewer_Something * vs=vum->texture;
    Interactor_GlobalInfo* g_info=vi->g_info;  

    Interactor_PickInfo ipi=g_info->ipi;
    Pose_Annotation_Tool_Prop*patp=(Pose_Annotation_Tool_Prop*)(vi->prop);

    if(g_info->mouse_button==VIEWER_MOUSE_BUTTON_LEFT&&g_info->mouse_action==VIEWER_PRESS)
    {
        if(ipi.vs!=NULL&&vum==ipi.vs->evolution)
        {

            printf("run time :%lf\n",g_info->run_time );

            if(vum->change_color<0.9)
            {
                patp->paa.is_play=0;
                patp->paa.begin_time=0;
                if(patp->paa.temp_m!=NULL)
                {
                    lb_matrix_free(patp->paa.temp_m);
                    patp->paa.temp_m=NULL;
                }
                if(patp->paa.temp_mi!=NULL)
                {
                    lb_matrix_free(patp->paa.temp_mi);
                    patp->paa.temp_mi=NULL;
                }
                if(patp->paa.temp_m1!=NULL)
                {
                    lb_matrix_free(patp->paa.temp_m1);
                    patp->paa.temp_m1=NULL;
                } 
                if(patp->paa.temp_m2!=NULL)
                {

                    if(patp->v_model!=NULL&&patp->v_scene!=NULL)
                    {

                        Viewer_Points* vp1=(Viewer_Points*)(patp->v_model->evolution);

                        vp1->mat->copy_data(vp1->mat,patp->paa.temp_m2);
                    }
                   
                    lb_matrix_free(patp->paa.temp_m2);
                    patp->paa.temp_m2=NULL;
                }
                patp->paa.n=0;

                vum->change_color=1.0;
            }
            else if(vum->change_color>0.9)
            {
                patp->paa.is_play=1;
                patp->paa.n=0;

                patp->paa.begin_time=g_info->run_time;
                  
                if(patp->v_model!=NULL&&patp->v_scene!=NULL)
                {
                    Viewer_Points* vp1=(Viewer_Points*)(patp->v_scene->evolution);
                    
                    LB_Matrix* temp_m1=(LB_Matrix*)malloc(sizeof(LB_Matrix));
                    lb_matrix_init_float(temp_m1,4,4);


                    patp->paa.temp_m=temp_m1;

                    temp_m1->copy_data(temp_m1,vp1->mat);

                    patp->paa.temp_mi=vp1->mat->inverse(vp1->mat); 


                    
                    vp1=(Viewer_Points*)(patp->v_model->evolution);
                    patp->paa.temp_m2=(LB_Matrix*)malloc(sizeof(LB_Matrix));
                    lb_matrix_init_float(patp->paa.temp_m2,4,4);
                    patp->paa.temp_m2->copy_data(patp->paa.temp_m2,vp1->mat);


                    LB_Matrix* temp_m=(LB_Matrix*) malloc(sizeof(LB_Matrix));
                    lb_matrix_init_float(temp_m,4,4);

                    temp_m->copy_data(temp_m,vp1->mat);
                    temp_m->mult(temp_m,patp->paa.temp_mi);
                    float* det=(float*)(temp_m->det(temp_m));
                    printf("%lf\n",*det);
                    free(det);
                    patp->paa.temp_m1=temp_m;

                    double mat_data[9];

                    for(int i=0;i<3;i++)
                    {
                        for(int j=0;j<3;j++)
                        {

                            mat_data[i*3+j]=((float*)(temp_m->data))[i*4+j];

                            printf("%lf ", mat_data[i*3+j]); 
                        }
                        printf("\n");

                    }

                    double *rota= get_rotation_from_matrix(mat_data);  

                    printf("%lf %lf %lf %lf\n",rota[0],rota[1],rota[2],rota[3]);
                    printf("%lf\n",SAFE_SQRT(rota[1]*rota[1]+rota[2]*rota[2]+rota[3]*rota[3]));
                    
                    patp->paa.angle=rota[0];
                    patp->paa.axis[0]=rota[1]; patp->paa.axis[1]=rota[2]; patp->paa.axis[2]=rota[3]; 
                    // get_matrix_from_rotation

                    free(rota);
                    vp1->mat->copy_data(vp1->mat,patp->paa.temp_m);

                    //lb_matrix_free(temp_m);
                }  

                vum->change_color=0.5;
            }
         
        }
    }
}



void record_pose_callback(Viewer_Intera*vi)
{
    Viewer_UI_Mesh* vum=(Viewer_UI_Mesh*)(vi->representation);
    Viewer_Something * vs=vum->texture;
    Interactor_GlobalInfo* g_info=vi->g_info;  

    Interactor_PickInfo ipi=g_info->ipi;
    Pose_Annotation_Tool_Prop*patp=(Pose_Annotation_Tool_Prop*)(vi->prop);


    //************
   
    

    if(g_info->mouse_button==VIEWER_MOUSE_BUTTON_LEFT&&g_info->mouse_action==VIEWER_PRESS)
    {
        if(ipi.vs!=NULL&&vum==ipi.vs->evolution)
        {

            //printf("record pose \n");
            vum->change_color=0.5;

            //printf("hell record\n"); 

            if(patp->v_model!=NULL&&patp->v_scene!=NULL)
            {
                // printf("hrell 1\n");

                Viewer_Points*vp=(Viewer_Points*)(patp->v_scene->evolution);

                LB_Matrix* mat=vp->mat->inverse(vp->mat);
                vp=(Viewer_Points*)(patp->v_model->evolution);
                mat->mult(mat,vp->mat);
                patp->n_poses=node_overlying(patp->n_poses,mat);


                int n_len=node_size(patp->n_poses);
                
                float* pos= record_position(n_len-1);

                Viewer_Something* vs1=viewer_add_button(patp->vw,"baise.jpg",pos,record_pose_small_parts,patp);
                free(pos);
                patp->n_poses->traits=vs1;
                vs1->prop=patp->n_poses;
                patp->voi->update_data(patp->voi); 
            } 
        }
    } 
    else if(g_info->mouse_button==VIEWER_RELEASE)
    {
            vum->change_color=1.0;   
    }

}
void patp_move_workpiece_cursor_callback(Viewer_Intera* vi)
{

	
    Interactor_GlobalInfo* g_info=vi->g_info;
    Pose_Annotation_Tool_Prop* patp=(Pose_Annotation_Tool_Prop*)(vi->representation);

    Viewer_Arcroll* va=(Viewer_Arcroll*)(patp->prop);
    if(patp->v_model==NULL)
    {
    	return;
    }
    Viewer_Points* vp=(Viewer_Points*)(patp->v_model->evolution);

    if(g_info->mouse_button==VIEWER_MOUSE_BUTTON_LEFT&&g_info->mouse_action==VIEWER_PRESS&& 
    	g_info->key_action==0&&patp->is_move_piece==1)
    {
		float tempx=g_info->mouse_coord[0]-patp->old_mouse_coord[0];
        float tempy=g_info->mouse_coord[1]-patp->old_mouse_coord[1];

        tempx=tempx*0.005;tempy=-tempy*0.005;

        LB_Matrix *temp_m=(LB_Matrix*)malloc(sizeof(LB_Matrix));
        lb_matrix_init_float(temp_m,4,4);
        float *data=(float*)(temp_m->data);



        data[0*4+0]=cos(tempx);data[0*4+1]=-sin(tempx)*sin(tempy);
        data[0*4+2]=sin(tempx)*cos(tempy);
        data[1*4+1]=cos(tempy);
        data[1*4+2]=sin(tempy);
        data[2*4+0]=-sin(tempx);
        data[2*4+1]=-cos(tempx)*sin(tempy);
        data[2*4+2]=cos(tempx)*cos(tempy);


 		LB_Matrix *temp_m1=(LB_Matrix*)malloc(sizeof(LB_Matrix));
        lb_matrix_init_float(temp_m1,4,4);

        data=(float*)(temp_m1->data);

        float* data1=(float*)(vp->mat->data);
        float* center=(float*)(patp->model->prop);
        for(int i=0;i<3;i++)
        {
            for(int j=0;j<3;j++)
            {

                data[i*4+3]+=center[j]*data1[i*4+j];
            }
            data[i*4+3]+=data1[i*4+3];
        }

       	LB_Matrix* temp_m1i= temp_m1->inverse(temp_m1);


        LB_Matrix *temp_m2=(LB_Matrix*)malloc(sizeof(LB_Matrix));
        lb_matrix_init_float(temp_m2,4,4);
        temp_m2->copy_data(temp_m2,va->mc->matrix);
        data=(float*)(temp_m2->data);
        data[0*4+3]=0;data[1*4+3]=0;data[2*4+3]=0;


        LB_Matrix* temp_m2i=temp_m2->inverse(temp_m2);
        temp_m1->mult(temp_m1,temp_m2);
        temp_m1->mult(temp_m1,temp_m);
        temp_m1->mult(temp_m1,temp_m2i);
        temp_m1->mult(temp_m1,temp_m1i);



        lb_matrix_free(temp_m2); 

        lb_matrix_free(temp_m2i);
       

        
      	temp_m1->mult(temp_m1,vp->mat);

        vp->mat->copy_data(vp->mat,temp_m1);
        if(patp->synchronized_mat!=NULL)
        {
            Viewer_Points* vp1 = ((Viewer_Points*)(patp->v_scene->evolution));
            LB_Matrix*temp_m3= vp1->mat->inverse(vp1->mat);
            temp_m3->mult(temp_m3,vp->mat); 

            patp->synchronized_mat->copy_data(patp->synchronized_mat,temp_m3);
            lb_matrix_free(temp_m3);

        }	

        lb_matrix_free(temp_m);
        lb_matrix_free(temp_m1);
        lb_matrix_free(temp_m1i);
    
    }
    else if (g_info->mouse_button==VIEWER_MOUSE_BUTTON_RIGHT&&g_info->mouse_action==VIEWER_PRESS&& 
    	g_info->key_action==0&&patp->is_move_piece==1)
    {
    	

    	float tempx=g_info->mouse_coord[0]-patp->old_mouse_coord[0];
        float tempy=g_info->mouse_coord[1]-patp->old_mouse_coord[1];	

        tempx=-tempx*0.001;tempy=tempy*0.001;

    	LB_Matrix *temp_m=(LB_Matrix*)malloc(sizeof(LB_Matrix));
        lb_matrix_init_float(temp_m,4,4);

        temp_m->mult(temp_m,va->mc->matrix);

       	LB_Matrix *temp_m1=(LB_Matrix*)malloc(sizeof(LB_Matrix));
        lb_matrix_init_float(temp_m1,4,4);
        float*data=(float*)(temp_m1->data);

        data[0*4+3]-=tempx;
        data[1*4+3]-=tempy;
        temp_m->mult(temp_m,temp_m1);
       	temp_m->mult(temp_m,va->mc->matrix_inverse);
       	temp_m->mult(temp_m,vp->mat);
       	vp->mat->copy_data(vp->mat,temp_m);
        if(patp->synchronized_mat!=NULL)
        {
            Viewer_Points* vp1 = ((Viewer_Points*)(patp->v_scene->evolution));
            LB_Matrix*temp_m3= vp1->mat->inverse(vp1->mat);
            temp_m3->mult(temp_m3,vp->mat); 

            patp->synchronized_mat->copy_data(patp->synchronized_mat,temp_m3);
            lb_matrix_free(temp_m3);
            
        }   
    }

    patp->old_mouse_coord[0]=g_info->mouse_coord[0];
    patp->old_mouse_coord[1]=g_info->mouse_coord[1];
}


void move_workpiece_callback(Viewer_Intera* vi)
{

	Viewer_UI_Mesh* vum=(Viewer_UI_Mesh*)(vi->representation);
    Viewer_Something * vs=vum->texture;
    Interactor_GlobalInfo* g_info=vi->g_info;  

    Interactor_PickInfo ipi=g_info->ipi;
    Pose_Annotation_Tool_Prop*patp=(Pose_Annotation_Tool_Prop*)(vi->prop);

    if(g_info->mouse_button==VIEWER_MOUSE_BUTTON_LEFT&&g_info->mouse_action==VIEWER_PRESS)
    {
        if(ipi.vs!=NULL&&vum==ipi.vs->evolution)
        {
        	if(vum->change_color<0.9)
        	{
        		patp->is_move_piece=0;
        		vum->change_color=1.0;
        	}
        	else if(vum->change_color>0.9)
        	{
        		patp->is_move_piece=1;
            	vum->change_color=0.5;
        	}
         
        }
    } 
    // else if(g_info->mouse_button==VIEWER_RELEASE)
    // {
    //         vum->change_color=1.0;   
    // }
}


Viewer_Something*  viewer_add_button(Viewer_World* vw,char * image ,float*pos , void (call_fun)(Viewer_Intera*),void* prop )
{
	Viewer_Something* re=NULL;

    //char uimesh[]="UI_Mesh";
    Node* n=vw->create_something(vw,"UI_Mesh");


    auto vs=(Viewer_Something*)(n->value);
    re=vs;

    auto vum=(Viewer_UI_Mesh*)(vs->evolution);
    //vs->disappear=1;
    vum->Data_rows=4;
    vum->Data_index_rows=2;
    vum->color_rows=vum->Data_rows;
    vum->Data=(float*)malloc(sizeof(float)*3*vum->Data_rows);

    vum->Data_index=(unsigned int*)malloc(sizeof(unsigned int)*4*vum->Data_index_rows);
    memmove(vum->Data,pos,sizeof(float)*vum->Data_rows*3);
    vum->Data_index[0]=3;vum->Data_index[1]=0;vum->Data_index[2]=1;vum->Data_index[3]=3;
    vum->Data_index[4]=3;vum->Data_index[5]=1;vum->Data_index[6]=2;vum->Data_index[7]=3;

    free_node(n);

    if(image!=NULL)
    {
        char texture[]="Texture";
        n=vw->create_something(vw,texture);
        vs=(Viewer_Something*)(n->value);
        free_node(node_reverse(n));
        Viewer_Texture*vt=(Viewer_Texture*)(vs->evolution);
        vum->texture=vs;
        vt->image_file=(char*)malloc(sizeof(char)*200);
        strcpy(vt->image_file,image);
        float *texcoords1=(float*)malloc(sizeof(float)*vum->Data_index_rows*7);
        texcoords1[0]=3;
        texcoords1[1]=0.0;texcoords1[2]=0.0;
        texcoords1[3]=1.0;texcoords1[4]=0.0;
        texcoords1[5]=0.0;texcoords1[6]=1.0;
        texcoords1[7]=3;
        texcoords1[8]=1.0;texcoords1[9]=0.0;
        texcoords1[10]=1.0;texcoords1[11]=1.0;
        texcoords1[12]=0.0;texcoords1[13]=1.0;
        vt->each_face_texture_coord=texcoords1; 

    }
    else {

        vum->color_rows=vum->Data_index_rows;
        vum->color=(float*)malloc(sizeof(float)*4*vum->color_rows);
        vum->color[0]=1.0; vum->color[1]=0.0; vum->color[2]=0.0; vum->color[3]=1.0;
        vum->color[4]=1.0; vum->color[5]=0.0; vum->color[6]=0.0; vum->color[7]=1.0;

    }
    
    n=vw->create_something(vw,"Intera");
    vs=(Viewer_Something*)(n->value);
    auto vi=(Viewer_Intera*)(vs->evolution);
    vi->representation=vum;
    vi->mouse_button_callback=call_fun;
    vi->prop=prop;
    free_node(n);
    return re;
}

static inline int is_remove_select_point( Interactor_PickInfo* ipi ,Pose_Annotation_Tool_Prop* patp)
{
    int mark=-1;
    for(int i=0;i<6;i++)
    {
        if(ipi->vs==patp->select_ps[i].vs&&ipi->marked_element==patp->select_ps[i].mark)
        {
            mark=i;
            break;
        }

    }
    if(mark==-1)
    {
        return 0;
    }
    for(int i=mark;i<6;i++)
    {
        if(i==5)
        {
            patp->select_ps[i].vs=NULL;
            patp->select_ps[i].mark=0;
        }
        else
        {
            patp->select_ps[i].vs=patp->select_ps[i+1].vs;
            patp->select_ps[i].mark=patp->select_ps[i+1].mark;    
        }
    }
    return 1;

}

static inline void update_select_points_viewer_data(Pose_Annotation_Tool_Prop* patp )
{
    if(patp->v_model!=NULL)
    {
        Viewer_Points* vp=(Viewer_Points*)(patp->v_model->evolution);
        vp->set_color(vp,0.9,0.3,0.7,1.0);
 
    } 
    if(patp->v_scene!=NULL)
    {
        Viewer_Points*vp=(Viewer_Points*)(patp->v_scene->evolution);

        vp->set_color(vp,0.3,0.9,0.4,1.0);
    }

    for(int i=0;i<6;i++)
    {
        if(patp->select_ps[i].vs!=NULL)
        {

            Viewer_Points*vp=(Viewer_Points*)(patp->select_ps[i].vs->evolution);
            vp->color[patp->select_ps[i].mark*4+0]=1.0; 
            vp->color[patp->select_ps[i].mark*4+1]=1.0; 
            vp->color[patp->select_ps[i].mark*4+2]=1.0; 
        } 

    } 

    patp->voi->update_data(patp->voi);
 
}





LB_Matrix* get_mat_form_select_points(Pose_Annotation_Tool_Prop*patp )
{

   

    float model_ps[3][3]={0},scene_ps[3][3]={0};
    
    int i=0,j=0;
    Viewer_Points* vp=NULL;
    for(int k=0;k<6;k++)
    {
        if(patp->select_ps[k].vs==patp->v_model)
        {
            vp=(Viewer_Points*)(patp->v_model->evolution);

            model_ps[i][0]=vp->Data[patp->select_ps[k].mark*3+0]; 
            model_ps[i][1]=vp->Data[patp->select_ps[k].mark*3+1]; 
            model_ps[i][2]=vp->Data[patp->select_ps[k].mark*3+2]; 
            i++;
        }
        else if(patp->select_ps[k].vs==patp->v_scene)
        {
            vp=(Viewer_Points*)(patp->v_scene->evolution);
            scene_ps[j][0]= vp->Data[patp->select_ps[k].mark*3+0]; 
            scene_ps[j][1]= vp->Data[patp->select_ps[k].mark*3+1]; 
            scene_ps[j][2]= vp->Data[patp->select_ps[k].mark*3+2]; 
            j++;
        }
    }  

    if(i<1||j<1)
    {
        return NULL;
    }
    LB_Matrix* re=(LB_Matrix*)malloc(sizeof(LB_Matrix));
    lb_matrix_init_float(re,4,4);

    float * data=(float*)(re->data);

    data[0*4+3]=scene_ps[0][0]-model_ps[0][0];    data[1*4+3]=scene_ps[0][1]-model_ps[0][1];    data[2*4+3]=scene_ps[0][2]-model_ps[0][2]; 
    
    //printf("%f %f %f\n",data[0*4+3],data[1*4+3],data[2*4+3] );

    if(i==3&& j==3)
    {
        double p1[3]={model_ps[1][0]-model_ps[0][0], model_ps[1][1]-model_ps[0][1], model_ps[1][2]-model_ps[0][2] },p2[3]={ model_ps[2][0]-model_ps[0][0], model_ps[2][1]-model_ps[0][1], model_ps[2][2]-model_ps[0][2] };
        double q1[3]={scene_ps[1][0]-scene_ps[0][0], scene_ps[1][1]-scene_ps[0][1], scene_ps[1][2]-scene_ps[0][2] },q2[3]={ scene_ps[2][0]-scene_ps[0][0], scene_ps[2][1]-scene_ps[0][1], scene_ps[2][2]-scene_ps[0][2] };

        for(int k=0;k<3;k++)
        {
            printf("model :%f %f %f\n",model_ps[k][0],model_ps[k][1],model_ps[k][2] );
        }
        for(int k=0;k<3;k++)
        {
            printf("scene :%f %f %f\n",scene_ps[k][0],scene_ps[k][1],scene_ps[k][2] );
        }


        normalize(p1,3);normalize(q1,3);

        double* n= out_product(p1,p2),*m=out_product(q1,q2);
        normalize(n,3);normalize(m,3);
        double *nxp= out_product(n,p1),*mxq=out_product(m,q1);

        LB_Matrix*temp_m1=(LB_Matrix*)malloc(sizeof(LB_Matrix));
        lb_matrix_init_float(temp_m1,4,4);

        data=(float*)(temp_m1->data);



        data[0*4+0]=p1[0] ;data[1*4+0]=p1[1] ;data[2*4+0]=p1[2];
        data[0*4+1]=n[0] ;data[1*4+1]=n[1] ;data[2*4+1]=n[2];
        data[0*4+2]=nxp[0] ;data[1*4+2]=nxp[1] ;data[2*4+2]=nxp[2];


        LB_Matrix*temp_m2=(LB_Matrix*)malloc(sizeof(LB_Matrix));
        lb_matrix_init_float(temp_m2,4,4); 
        data=(float*)(temp_m2->data);



        data[0*4+0]=q1[0] ;data[1*4+0]=q1[1] ;data[2*4+0]=q1[2];
        data[0*4+1]=m[0] ;data[1*4+1]=m[1] ;data[2*4+1]=m[2];
        data[0*4+2]=mxq[0] ;data[1*4+2]=mxq[1] ;data[2*4+2]=mxq[2];

        LB_Matrix* temp_mi= temp_m1->inverse(temp_m1);

        LB_Matrix* temp_m3=(LB_Matrix*)malloc(sizeof(LB_Matrix));
        lb_matrix_init_float(temp_m3,4,4);
        data=(float*)(temp_m3->data);
        data[0*4+3]=model_ps[0][0]; data[1*4+3]=model_ps[0][1]; data[2*4+3]=model_ps[0][2];

        LB_Matrix* temp_m3i=temp_m3->inverse(temp_m3);

        re->mult(re,temp_m3); 
       
        re->mult(re,temp_m2);re->mult(re,temp_mi);
        re->mult(re,temp_m3i);

        lb_matrix_free(temp_m3); lb_matrix_free(temp_m3i);
         
        lb_matrix_free(temp_mi);
        lb_matrix_free(temp_m1);lb_matrix_free(temp_m2);
        SAFE_FREE(nxp);SAFE_FREE(mxq);
        SAFE_FREE(n);SAFE_FREE(m);

    }
    //float * data=(float*)(re->data);
 
    return re;
}


void patp_key_callback(Viewer_Intera*vi)
{
 	Interactor_GlobalInfo* g_info=vi->g_info;
    Pose_Annotation_Tool_Prop* patp=(Pose_Annotation_Tool_Prop*)(vi->representation);


    if(g_info->key_action==VIEWER_PRESS&&g_info->key==VIEWER_KEY_D&&g_info->key_mods==VIEWER_MOD_CONTROL)
    {
    	//printf("here cheng\n");
    	for(Node*nit=patp->n_vums;nit!=NULL;nit=(Node*)(nit->Next))
    	{
    		Viewer_Something* vs= (Viewer_Something*)(nit->value);
    		vs->disappear=1;

    	}	
    }
    else if(g_info->key_action==VIEWER_PRESS&&g_info->key==VIEWER_KEY_B&&g_info->key_mods==VIEWER_MOD_CONTROL)
    {
		for(Node*nit=patp->n_vums;nit!=NULL;nit=(Node*)(nit->Next))
    	{
    		Viewer_Something* vs= (Viewer_Something*)(nit->value);
    		vs->disappear=0;

    	}	

    }
    else if(patp->is_input==1&&g_info->key_action==VIEWER_PRESS&&g_info->key==VIEWER_KEY_DELETE)
    {
        Viewer_Texts* vtexts=(Viewer_Texts*)(patp->input_str->evolution);
        
        int len =wcslen(vtexts->str);

       // int len=wcslen(vtexts->str);
        vtexts->xy[0]=-0.5-len*0.022;

        if(len>0)
        {


            vtexts->str[len-1]='\0';
        }

    }
    else if(g_info->key_action==VIEWER_PRESS&&g_info->key==VIEWER_KEY_S)
    {

        if(g_info->ipi.vs!=NULL)
        {
            //printf("%s name_id:%d marked_id: %d\n",g_info->ipi.vs->name,g_info->ipi.vs->name_id,g_info->ipi.marked_element);

            if(strcmp(g_info->ipi.vs->name,"Points")==0)
            {
                Viewer_Points* vp=(Viewer_Points*)(g_info->ipi.vs->evolution);


                // printf("piont :%lf %lf %lf\n",vp->Data[g_info->ipi.marked_element*3+0], 
                //     vp->Data[g_info->ipi.marked_element*3+1],vp->Data[g_info->ipi.marked_element*3+2] );
            
                if(!is_remove_select_point(&(g_info->ipi)   , patp))
                {
                    int len=0;
                    for(int i=0;i<6;i++)
                    {
                        if(patp->select_ps[i].vs!=NULL)
                        {
                            len++;
                        } 
                    } 
                    if(len<6)
                    {
                        patp->select_ps[len].vs=g_info->ipi.vs;
                        patp->select_ps[len].mark= g_info->ipi.marked_element;
                        len++;
                    }
                }
                update_select_points_viewer_data(patp);



            } 
        }
    }
    else if(g_info->key_action==VIEWER_PRESS&&g_info->key==VIEWER_KEY_C)
    {
        if(patp->v_model==NULL||patp->v_scene==NULL)
        {
            return;
        }
        
        LB_Matrix* mat= get_mat_form_select_points(patp);

        if(mat!=NULL)
        {
            mat->print_self(mat);


            Viewer_Points* vp1=(Viewer_Points*)(patp->v_model->evolution);
            Viewer_Points* vp2=(Viewer_Points*)(patp->v_scene->evolution);

            vp1->mat->copy_data(vp1->mat,vp2->mat);


            vp1->mat->mult(vp1->mat,mat);
            lb_matrix_free(mat);

            for(int i=0;i<6;i++)
            {
                patp->select_ps[i].vs=NULL;
                patp->select_ps[i].mark=0;
            }

            update_select_points_viewer_data(patp);

        }
    }
}


void patp_char_callback(Viewer_Intera*vi ,unsigned int c)
{

    Interactor_GlobalInfo* g_info=vi->g_info;
    Pose_Annotation_Tool_Prop* patp=(Pose_Annotation_Tool_Prop*)(vi->representation);
    char cc=c;
    wchar_t wc[5]={0};
    mbstowcs(wc,&cc,sizeof(unsigned char));
    if(patp->input_str!=NULL&&patp->is_input==1)
    {
        Viewer_Something* vs=patp->input_str;
        Viewer_Texts* vtexts=(Viewer_Texts*)(vs->evolution);

        int len=wcslen(vtexts->str);
        vtexts->xy[0]=-0.5-len*0.022;

        wcscat(vtexts->str,wc); 
        //printf("%ls\n",vtexts->str);  
    }

   
    //printf("dsfd:%ls\n",wc);
}
void patp_animation_drop_callback(Viewer_Intera* vi)
{

    Interactor_GlobalInfo* g_info=vi->g_info;
    Pose_Annotation_Tool_Prop* patp=(Pose_Annotation_Tool_Prop*)(vi->representation);

    if(patp->v_model==NULL||patp->v_scene==NULL)
    {
        return;
    }
    char model_name[200]={0};
    char scene_name[200]={0};
    float data[16]={0};
    for(int i=0;i<g_info->drop_count;i++)
    {

        printf("%s\n",g_info->paths[i]);

#ifdef _WIN32
        FILE *infile;
        fopen_s(&infile,g_info->paths[i],"r");
#else
        FILE *infile=fopen(g_info->paths[i],"r");
#endif
        if(!infile)
        {
            printf("cant't open this file\r\n");
            continue;
        }
        fseek(infile,0,SEEK_SET);
        char str[300]={0};

        while(!feof(infile))
        {
            fgets(str,300,infile);
            
           

            //printf("once %s\n",str);
            int num=sscanf(str,"%s %s %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f", 
                model_name,scene_name, &(data[0]), &(data[1]), &(data[2]), &(data[3]),  
                 &(data[4]), &(data[5]), &(data[6]), &(data[7]),
                 &(data[8]), &(data[9]), &(data[10]), &(data[11]),
                 &(data[12]), &(data[13]), &(data[14]), &(data[15]));
           

            printf("scene_name:%s\n",patp->scene_name);
            if(num==18)
            {
               
                if(strcmp(model_name,patp->model_name)==0&&strcmp(scene_name,patp->scene_name)==0)
                {
                    LB_Matrix* mat=(LB_Matrix*)malloc(sizeof(LB_Matrix));
                    lb_matrix_init_float(mat,4,4);

                    memmove(mat->data,data,sizeof(float)*16 );
                    patp->n_poses=node_overlying(patp->n_poses,mat);
                    int n_len=node_size(patp->n_poses);
                    float* pos=record_position(n_len-1);

                    Viewer_Something* vs1=viewer_add_button(patp->vw,"baise.jpg",pos,record_pose_small_parts,patp);

                    free(pos);
                    patp->n_poses->traits=vs1;
                    vs1->prop=patp->n_poses;
                    patp->voi->update_data(patp->voi);


                }
            }
            memset(str,0,sizeof(char)*300);
           
        }
        fclose(infile);
    }
    //printf("%d\n",g_info->drop_count);

}


void patp_animation_callback(Viewer_Intera *vi)
{
    Interactor_GlobalInfo* g_info=vi->g_info;
    Pose_Annotation_Tool_Prop* patp=(Pose_Annotation_Tool_Prop*)(vi->representation);
    if(patp->v_model==NULL||patp->v_scene==NULL)
    {
        return;
    }
    float cha= (g_info->run_time- patp->paa.begin_time);
    if(patp->paa.is_play==0||patp->paa.angle==0||patp->paa.temp_m==NULL||cha >4.0  )
    {
        return;
    } 

    if( cha> patp->paa.n*0.04 )
    {

        patp->paa.n++;
        double* mat_data=get_matrix_from_rotation(patp->paa.angle*0.01*patp->paa.n,patp->paa.axis  );

        LB_Matrix* temp_m=(LB_Matrix*)malloc(sizeof(LB_Matrix));
        lb_matrix_init_float(temp_m,4,4);

        for(int i=0;i<3;i++)
        {
            for(int j=0;j<3;j++)
            {

                ((float*)(temp_m->data))[i*4+j]=mat_data[i*3+j];
            }

        }

        ((float*)(temp_m->data))[0*4+3]=0.01*patp->paa.n* (((float*)(patp->paa.temp_m1->data ))[0*4+3]);
        ((float*)(temp_m->data))[1*4+3]=0.01*patp->paa.n* (((float*)(patp->paa.temp_m1->data ))[1*4+3]);
        ((float*)(temp_m->data))[2*4+3]=0.01*patp->paa.n* (((float*)(patp->paa.temp_m1->data ))[2*4+3]);

        Viewer_Points* vp1=(Viewer_Points*)(patp->v_model->evolution);

        vp1->mat->identity(vp1->mat);
        vp1->mat->mult(vp1->mat, temp_m);
        vp1->mat->mult(vp1->mat,patp->paa.temp_m);
        

        lb_matrix_free(temp_m);

        free(mat_data);

    }


    // printf("%lf \n",(g_info->run_time- patp->paa.begin_time)  );


}

int main()
{


	//test_save();

	Viewer_World_Manager vwm;
	viewer_world_manager_init(&vwm);
	Viewer_World* vw=vwm.create_world(&vwm,"中原动力位姿标注");


	Viewer_Opengl_Interpreter voi;
    
	Viewer_Arcroll* va= add_default_somethings(vw);
	// Mesh mesh;
	// Mesh_init(&mesh);
	// _ReadPly_(&mesh,"92.ply");


	// Node*n=vw->create_something(vw,"Points");

	// auto vs=(Viewer_Something*)(n->value);
	// auto vp=(Viewer_Points*)(vs->evolution);

	// printf("num v: %d\n",mesh.num_v(&mesh));
	// vp->Data_rows=mesh.num_v(&mesh);
	// printf("%d \n",vp->Data_rows);

	// vp->Data=(float*)malloc(sizeof(float)*3*vp->Data_rows);
	// vp->set_color(vp,0.5,0.3,0.7,1.0);
	// int i=0;


	// for(auto vit=mesh.v_begin(&mesh);vit!=mesh.v_end(&mesh);vit++)
	// {
	// 	if(i>=vp->Data_rows)
	// 	{
	// 		printf("i %d\n",i);
	// 		break;
	// 	}
	// 	for(int j=0;j<3;j++)
	// 	{
	// 		vp->Data[i*3+j]=quote(vit)->point[j];
	// 	}
	// 	i++;
	// }
	// //vp->color[(i-1)*4]=1.0;

	// vp->pointsize=7.0;
	// free_node(n);


	

	Pose_Annotation_Tool_Prop* patp=(Pose_Annotation_Tool_Prop*)malloc(sizeof(Pose_Annotation_Tool_Prop));
	pose_annotation_tool_prop_init(patp);
	va->prop=patp;patp->prop=va;
	patp->vw=vw;
	patp->voi=&voi;

	float pos_1[12]={-0.9,0.75,1.0,
        -0.6,0.75,1.0,
        -0.6,0.95,1.0,
        -0.9,0.95,1.0
    };
	Viewer_Something* vs= viewer_add_button(vw,"select_workpiece.jpg",pos_1,select_workpiece_call_fun,patp);

	patp->n_vums=node_overlying(patp->n_vums,vs);


	float pos_2[12]={-0.9,0.52,1.0,
        -0.6,0.52,1.0,
        -0.6,0.73,1.0,
        -0.9,0.73,1.0
    };	

	vs=viewer_add_button(vw,"select_scene.jpg",pos_2,select_scene_call_fun,patp);

	patp->n_vums=node_overlying(patp->n_vums,vs);


	float pos_3[12]={-0.9,0.29,1.0,
        -0.6,0.29,1.0,
        -0.6,0.49,1.0,
        -0.9,0.49,1.0
    };	

	vs=viewer_add_button(vw,"output_pose.jpg",pos_3,output_pose_call_fun,patp);

	patp->n_vums=node_overlying(patp->n_vums,vs);
	float pos_4[12]={-0.9,0.04,1.0,
        -0.6,0.04,1.0,
        -0.6,0.26,1.0,
        -0.9,0.26,1.0
    };	

	vs=viewer_add_button(vw,"input_matrix.jpg",pos_4,input_matrix_callback,patp);
	patp->n_vums=node_overlying(patp->n_vums,vs);

	float pos_5[12]={-0.9,-0.19,1.0,
        -0.6,-0.19,1.0,
        -0.6,0.03,1.0,
        -0.9,0.03,1.0
    };	
	vs=viewer_add_button(vw,"ppf.jpg",pos_5,ppf_recommand_callback,patp);

	patp->n_vums=node_overlying(patp->n_vums,vs);




	float pos_6[12]={-0.9,-0.42,1.0,
        -0.6,-0.42,1.0,
        -0.6,-0.20,1.0,
        -0.9,-0.20,1.0
    };	
	vs=viewer_add_button(vw,"record_pose.jpg",pos_6,record_pose_callback,patp);

	patp->n_vums=node_overlying(patp->n_vums,vs);


	float pos_7[12]={-0.9,-0.65,1.0,
        -0.6,-0.65,1.0,
        -0.6,-0.43,1.0,
        -0.9,-0.43,1.0
    };	
	vs=viewer_add_button(vw,"move_workpiece.jpg",pos_7,move_workpiece_callback,patp);
	patp->n_vums=node_overlying(patp->n_vums,vs);


	float pos_8[12]={-0.9,-0.88,1.0,
        -0.6,-0.88,1.0,
        -0.6,-0.66,1.0,
        -0.9,-0.66,1.0
    };	
	vs=viewer_add_button(vw,"play_ani.jpg",pos_8,paly_ani_callback,patp);
	patp->n_vums=node_overlying(patp->n_vums,vs);


	
    // float center[3]={0.9-n*0.07,0.9-col*0.07,1.0 }; 

	// float pos_9[12]={0.9-0.03,0.9-0.03,1.0,
 //        0.9+0.03,0.9-0.03,1.0,
 //        0.9+0.03,0.9+0.03  ,1.0,
 //        0.9-0.03,0.9+0.03,1.0
 //    };
 //    vs=viewer_add_button(vw,"baise.jpg",pos_9,NULL,patp);

	// patp->n_vums=node_overlying(patp->n_vums,vs);



	
	// float pos_7[12]={0.5,-0.51,1.0,
 //        0.99,-0.51,1.0,
 //        0.99,0.61,1.0,
 //        0.5,0.61,1.0
 //    };


	// vs=viewer_add_button(vw,"baise.jpg",pos_7,NULL,patp);
	// patp->n_vums=node_overlying(patp->n_vums,vs);

  	Node*n=vw->create_something(vw,"Texts");
    vs=(Viewer_Something*)(n->value);
    patp->input_str=vs;
    Viewer_Texts* vtexts=(Viewer_Texts*)(vs->evolution);
    vtexts->str=(wchar_t*)malloc(sizeof(wchar_t)*900);
    memset(vtexts->str,0,sizeof(wchar_t)*900);
    wcscpy(vtexts->str,L"                中原动力位姿标注");

     

    int len=wcslen(vtexts->str);
    vtexts->text_alpha=(float*)malloc(sizeof(float)*len);
    memset(vtexts->text_alpha,0,sizeof(float)*len);
    vtexts->xy[0]=-0.6;vtexts->xy[1]=0.85;vtexts->scale=1.3;
    free_node(n);
   
    // printf("%d\n",21/10);




	n=vw->create_something(vw,"Intera");

 	vs=(Viewer_Something*)(n->value);

	Viewer_Intera*vi=(Viewer_Intera*)(vs->evolution);
    
 	vi->representation=(void*)patp;
    
 	vi->cursor_position_callback=patp_move_workpiece_cursor_callback;
 //    //vi->scroll_callback=viewer_Arcroll_scroll_callback;
 //    //vi->mouse_button_callback=viewer_Arcroll_mouse_button_callback;
 	vi->key_callback=patp_key_callback; 
     vi->animation=patp_animation_callback ;
    vi->char_callback=patp_char_callback;
    vi->drop_callback=patp_animation_drop_callback;
 //    vi->prop=patp;

 	free_node(n);

    // n=vw->create_something(vw,"Points");
    // vs= (Viewer_Something*)(n->value);
    // auto vp=(Viewer_Points*)(vs->evolution);
    // vp->Data_rows=0;
    // vp->Data=NULL;

    // vp->pointsize=27.0; 

    // patp->v_sp=vs;

    // free_node(n);


	// Mesh_free(&mesh);
	viewer_opengl_interpreter_initn(&voi,&vwm);
	voi.interpreter(&voi);


	viewer_opengl_interpreter_free(&voi);
	viewer_world_manager_free(&vwm); 
	
	pose_annotation_tool_prop_free(patp);
	printf("end\n");
	return 0;
}
